#ifndef GRID_REFINEMENT_NODE_HPP
#define GRID_REFINEMENT_NODE_HPP

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <visualization_msgs/MarkerArray.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <Eigen/Dense>
#include <unordered_map>
#include <vector>
#include <chrono>
#include <string>
#include <sstream>

class GridRefinementNode {
public:
    GridRefinementNode(ros::NodeHandle& nh);
    ~GridRefinementNode() = default;

private:
    // ROS相关
    ros::NodeHandle nh_;
    ros::Subscriber ground_sub_;
    ros::Subscriber nonground_sub_;
    ros::Publisher refined_ground_pub_;
    ros::Publisher refined_nonground_pub_;
    ros::Publisher grid_height_pub_;

    // 网格参数
    double grid_resolution_;
    double grid_x_min_, grid_x_max_;
    double grid_y_min_, grid_y_max_;

    // 数据存储
    pcl::PointCloud<pcl::PointXYZI>::Ptr ground_points_;
    pcl::PointCloud<pcl::PointXYZI>::Ptr nonground_points_;
    
    // 使用字符串作为网格索引，与Python版本逻辑一致
    std::unordered_map<std::string, std::vector<double>> ground_grid_;
    std::unordered_map<std::string, double> grid_avg_heights_;

    // 性能监控
    std::chrono::high_resolution_clock::time_point start_time_;
    std::vector<double> processing_times_;
    size_t max_history_size_;

    // 回调函数
    void groundCallback(const sensor_msgs::PointCloud2::ConstPtr& msg);
    void nongroundCallback(const sensor_msgs::PointCloud2::ConstPtr& msg);

    // 核心处理函数
    void processGroundPoints();
    void processNongroundPoints();

    // 辅助函数
    std::string getGridIndex(double x, double y);
    void preallocateGrids();
    void publishPointCloud(const pcl::PointCloud<pcl::PointXYZI>::Ptr& cloud, 
                          ros::Publisher& publisher, const std::string& frame_id);
    void publishGridHeightVisualization();
    void logProcessingTime(const std::string& operation_name);
    void monitorPerformance(double processing_time);
};

#endif // GRID_REFINEMENT_NODE_HPP 